#ifndef COLORTRACKER_H_
#define COLORTRACKER_H_

#include "Driver.h"
#include "../utilities/Constants.h"
#include "../utilities/ObjPosition.h"

#include <string>
#include <boost/thread.hpp>

namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}


class ColorTracker {

private:
	Driver* m_driver;
	double m_bins[IMAGE_WIDTH];

	ros::NodeHandle m_node_handler;
	ros::Subscriber m_subscriber;
	ros::Rate* m_loop_rate;	

	std::mutex mutex_;

	double calcHorizontalOrientation(int xCord);
	double calcSpeed(double distToObj, double timeToMove);
	ObjPosition safeRead();
	void parseMsg(std::string msg, int& x, int& y, float& dist);

public:
	ColorTracker(Driver* driver);
	virtual ~ColorTracker();
	bool track();

	void positionSetter(const std_msgs::String::ConstPtr& msg);	
};

#endif /* COLORTRACKER_H_ */

/*
 * 1. get position and distance
 * 2. if no object found, turn right and start again
 * 3. a. if close enough to the object, stop and notify rover manager
 *    b. else, drive forward
 * 4. correct horizontal orientation
 */
